/**
 * @author Yunfeng Bai
 *
 * 2012-6-27
 */
package simulation;

import java.util.ArrayList;
import java.util.List;
import java.util.Random;

import javax.vecmath.Matrix3d;
import javax.vecmath.Vector3d;

import rigidbody.RigidBody;
import util.Util;
import collision.Collision;


public class ImpulseConstraintSolver implements ConstraintSolver
{
	private static double ELASTIC_RESTITUTION_PARAMETER = 0.0;
	private static double FRICTION_PARAMETER = 0.3;
	private static int MAX_COLLISION_ITERATION = 5;
	private static int MAX_COLLISION_PAIR_ITERATION = 3;
	private static int MAX_CONTACT_ITERATION = 10;
	private static int MAX_CONTACT_PAIR_ITERATION = 3;
	protected CollisionDetector detector = new CollisionDetector();
	protected ContactGraph contactGraph = new ContactGraph();
	protected boolean collisionResolved;
	
	public static long collisionTime = 0;
	public static long solveImpulseTime = 0;
	public static long backupAndIntegrationTime = 0;
	public static long OBBCollisionTime = 0;
	public static long getPenetrationPointsTime = 0;
	public static long initOBBTreeTime = 0;
	public static long updateOBBMatrixTime = 0;
	
	private Random random = new Random();
	
	@Override
	public boolean solveVelocities(Simulator sim)
	{
		solveCollisions(sim);
		
		sim.updateVelocities(true);
		
		// 5. Resolve contacts.
		if (!collisionResolved)
			solveContacts(sim);
		
		return false;
	}
	
	public boolean solveCollisions(Simulator sim)
	{
		List<RigidBody> bodies = sim.getBodies();
		double timestep = sim.getTimestep();
		int bodyCount = bodies.size();
		
		int randomIndexList[] = new int[bodyCount];
		int i;
		
		for (i = 0; i < bodyCount; i++)
			randomIndexList[i] = i;
		
		// Randomly swap the indices.
//		for (i = 0; i < bodyCount; i++)
//		{
//			int id1 = random.nextInt(bodyCount);
//			int id2 = random.nextInt(bodyCount);
//			int temp = randomIndexList[id1];
//			randomIndexList[id1] = randomIndexList[id2];
//			randomIndexList[id2] = temp;
//		}
		
		long time1 = 0, time2 = 0, time3 = 0;
		collisionTime = 0;
		solveImpulseTime = 0;
		backupAndIntegrationTime = 0;
		OBBCollisionTime = 0;
		getPenetrationPointsTime = 0;
		initOBBTreeTime = 0;
		updateOBBMatrixTime = 0;
		
		time3 = System.nanoTime();
		
		for (i = 0; i < MAX_COLLISION_ITERATION; i++)
		{
			collisionResolved = true;
			
			for (int j = 0; j < bodyCount; j++)
				for (int k = 0; k < j; k++)
				{
					RigidBody b1 = bodies.get(randomIndexList[j]), b2 = bodies.get(randomIndexList[k]);
					for (int l = 0; l < MAX_COLLISION_PAIR_ITERATION; l++)
					{
						// 1. Backup velocity and position.
						// 2. Integrate velocity.
						// 3. Integrate position.
						// 4. Detect collision with integrated velocity and position.
						// 5. Add solved impulse back to original velocity.
						// 6. Restore position.
						time1 = System.nanoTime();
						b1.backupPosition();
						b2.backupPosition();
						b1.backupVelocity();
						b2.backupVelocity();
						
						b1.integrateVelocity(timestep);
						b2.integrateVelocity(timestep);
						b1.integratePosition(timestep);
						b2.integratePosition(timestep);
						time2 = System.nanoTime();
						backupAndIntegrationTime += time2 - time1;
						
						time1 = System.nanoTime();
						List<Collision> collisions = RigidBody.getCollisionsBetween(b1, b2, true);
						time2 = System.nanoTime();
						collisionTime += time2 - time1;
						
						if (collisions.size() == 0)
						{
							time1 = System.nanoTime();
							b1.restoreVelocity();
							b2.restoreVelocity();
							b1.restorePosition();
							b2.restorePosition();
							time2 = System.nanoTime();
							backupAndIntegrationTime += time2 - time1;
							break;
						}
						else
						{
							collisionResolved = false;
							// Solve the impulse when there are collisions.
							time1 = System.nanoTime();
							for (Collision c : collisions)
							{
								Vector3d impulse = solveImpulse(c, ELASTIC_RESTITUTION_PARAMETER);
								c.object1.applyImpulseToCurrent(impulse, c.positionInWorld);
								impulse.scale(-1);
								c.object2.applyImpulseToCurrent(impulse, c.positionInWorld);
								break;
							}
							time2 = System.nanoTime();
							solveImpulseTime += time2 - time1;
						}
						
						time1 = System.nanoTime();
						b1.restoreVelocity();
						b2.restoreVelocity();
						b1.restorePosition();
						b2.restorePosition();
						time2 = System.nanoTime();
						backupAndIntegrationTime += time2 - time1;
					}
				}
			
			if (collisionResolved) break;
		}
		
		if (i > 0)
		{
			time1 = System.nanoTime();
			Util.trace("Iteration " + i);
			Util.traceTime("Init OBB tree time ", initOBBTreeTime);
			Util.traceTime("  update OBB matrices time ", updateOBBMatrixTime);
			Util.traceTime("OBB collision time ", OBBCollisionTime);
			Util.traceTime("Get penetration points time ", getPenetrationPointsTime);
			Util.traceTime("Collision detection time ", collisionTime);
			Util.traceTime("Collision resolve time ", (time1 - time3));
			Util.trace("collision process iteration: " + i);
		}
		
		return collisionResolved;
	}

	private Vector3d solveImpulse(Collision c, double restitutionParameter)
	{
		Vector3d urel = RigidBody.getRelativeVelocityAtPoint(c.positionInWorld, c.object1, c.object2);
		double ureln = urel.dot(c.normal);
		
		Matrix3d ka = c.object1.getK(c.normal, c.positionInWorld);
		Matrix3d kb = c.object2.getK(c.normal, c.positionInWorld);
		
		// Calculate the impulse with friction.
		Vector3d urelnVec = new Vector3d(c.normal);
		urelnVec.scale(ureln);
		Vector3d tmp = new Vector3d(urelnVec);
		tmp.scale(-restitutionParameter);
		tmp.sub(urel);
		
		Matrix3d k = new Matrix3d(ka);
		k.add(kb);
		Matrix3d kinv = new Matrix3d(k);
		kinv.invert();
		
		Vector3d j = new Vector3d();
		kinv.transform(tmp, j);
		
		Vector3d jt = new Vector3d(c.normal);
		jt.scale(j.dot(c.normal));
		jt.sub(j, jt);
		
		if (jt.length() <= Math.abs(FRICTION_PARAMETER * j.dot(c.normal)))
		{
			// Inside friction cone, static friction.
		}
		else
		{
			// Outside friction cone, dynamic friction.
			Vector3d t = new Vector3d(jt);
			t.normalize();
			tmp.set(t);
			tmp.scale(FRICTION_PARAMETER);
			tmp.sub(c.normal, tmp);
			k.transform(tmp);
			double denom = c.normal.dot(tmp);
			double jn = -(restitutionParameter + 1) * ureln / denom;
			
			jt.scale(FRICTION_PARAMETER * jn, t);
			j.scale(jn, c.normal);
			j.sub(j, jt);
		}
		
		return j;
	}
	
	public void solveContacts(Simulator sim)
	{
		contactGraph.updateGraph();
		contactGraph.updateOrder();
		
		// If there are still contacts left to process, we'll make the
		// last iteration shock propagation.
		List<RigidBody> bodies = contactGraph.getOrderedRigidBodies();
		
		List<Vector3d> originalVecs = new ArrayList<Vector3d>();
		for (RigidBody b : bodies)
		{
			originalVecs.add((Vector3d)b.getLinearMomentum().clone());
		}
		
		double timestep = sim.getTimestep();
		
		int bodyCount = bodies.size();
		for (int i = 0; i < bodyCount; i++)
			for (int j = 0; j < i; j++)
			{
				RigidBody b1 = bodies.get(i), b2 = bodies.get(j);
				// b2 is pinned. Backup it's immobile state.
				boolean immobileState = b2.isImmobile();
				b2.setIsImmobile(true);
				
				if (b1.isImmobile() && b2.isImmobile())
				{
					// b1 and b2 might have collisions. This rare case will happen.
					// Consider three objects, a, b, c,
					// a.y < b.y < c.y, and b & c are in the same component,
					// which positions are in front of a.
					// a is immobile, b is going up and c is going down.
					// After cluster b&c is resolved, b goes down, and because b comes before a,
					// b2 (= b) is set immobile and will collide with a (which is immobile as well).
					b2.setIsImmobile(immobileState);
					continue;
				}
				
				for (int l = 0; l < MAX_CONTACT_PAIR_ITERATION; l++)
				{
					b1.backupPosition();
					b1.backupVelocity();
					b1.integratePosition(timestep);
					
					List<Collision> collisions = RigidBody.getCollisionsBetween(b1, b2, true);
						
					if (collisions.size() > 0)
					{
						for (Collision c : collisions)
						{
							Vector3d impulse = solveImpulse(c, 0);
							c.object1.applyImpulseToCurrent(impulse, c.positionInWorld);
							impulse.scale(-1);
							c.object2.applyImpulseToCurrent(impulse, c.positionInWorld);
							
							break;
						}
					}
					else
					{
						b1.restorePosition();
						b1.restoreVelocity();
						break;
					}
					
					b1.restorePosition();
					b1.restoreVelocity();
				}
				b2.setIsImmobile(immobileState);
			}
	}
	
	@Override
	public void rigidBodyAdded(Simulator sim)
	{
		try
		{
			contactGraph.init(sim.getBodies(), sim.getTimestep());
		} catch (Exception e)
		{
			e.printStackTrace();
		}
	}
}
